import numpy as np
import cv2

class posGeter():
	def getPostion(self,xs,ys,Ymax = 0):
		print("feature get:xs[%s] ys:[%s] Ymax:[%s]"%(xs,ys,Ymax))
		# if Ymax != 0:
		# 	ys = Ymax-ys
		# xs -= 1000
		#以上转换到以车头中心为原点的坐标


		#find the most further point

		x = xs[0]
		y = ys[0]

		dist = np.square(x-xs) + np.square(y - ys)

		print(dist.shape)
		print(dist)

		index = np.argmax(dist)

		#print(xs[index],ys[index])

		p1 = Point(xs[0],ys[0])
		p2 = Point(xs[index],ys[index])

		print(xs)
		xs = np.delete(xs, [0, index])
		ys = np.delete(ys, [0, index])
		print(xs)

		p3 = Point(xs[0],ys[0])
		p4 = Point(xs[1],ys[1])

		line1 = Line(p1, p2)
		line2 = Line(p3, p4)

		angle = 0
		angle = (line1.getAngle()+line2.getAngle())/2
		if angle > np.pi*0.8:
			print('angle - 0.5pi')
			angle -= np.pi*0.5


		Pc = self.GetCrossPoint(line1, line2)
		print("Cross point:", Pc.x, Pc.y)

		x = Pc.x
		y = Pc.y

		print("centerGet:[x:%d y:%d angle:%2f]"%(x,y,angle))

		return self.convert2cm([x,y,angle])

	def convert2cm(self,centerPara):
		x = centerPara[0]
		y = centerPara[1]
		angle = centerPara[2]

		newAngle = angle
		newX = (x*2-2000)/50
		newY = 66 + (1500-(2*y-800))/50 + 10.4

		print("convert2cm ans:%s"%[newX,newY,newAngle])
		return [newX,newY,newAngle]


	def getCarPostion(self,centerPara):
		x = centerPara[0]
		y = centerPara[1]
		angle = centerPara[2]

		newAngle =  angle-np.pi / 2
		dy = 1250+730+650-y
		dx = x - 500

		angle = angle - np.pi/2

		newX = - dy * np.sin(angle) - dx * np.cos(angle)
		newY = - dy*np.cos(angle) + dx * np.sin(angle)
		print("getCarPostion ans:%s"%[newX,newY,newAngle])
		return [newX,newY,newAngle]




	def GetLinePara(self,line):
		line.a = line.p1.y - line.p2.y
		line.b = line.p2.x - line.p1.x
		line.c = line.p1.x * line.p2.y - line.p2.x * line.p1.y

	def GetCrossPoint(self,l1, l2):
		self.GetLinePara(l1)
		self.GetLinePara(l2)
		d = l1.a * l2.b - l2.a * l1.b
		p = Point()
		p.x = (l1.b * l2.c - l2.b * l1.c) * 1.0 / d
		p.y = (l1.c * l2.a - l2.c * l1.a) * 1.0 / d
		return p

class Point(object):
	def __init__(self, x=0, y=0):
		print("point creat x:%d y:%d"%(x,y))
		self.x = x
		self.y = y

class Line(object):
	def __init__(self, p1, p2):
		self.p1 = p1
		self.p2 = p2

	def getAngle(self):

		angle = 0
		if self.p1.x == self.p2.x:
			angle = np.pi / 2
		else:
			angle = - np.arctan((self.p1.y - self.p2.y)/(self.p1.x - self.p2.x))
		if angle < 0:
			angle += np.pi
		if angle > np.pi:
			angle -= np.pi

		print("line x1:%2f,y1:%2f x2:%2f,y2:%2f angle:%2f"%(self.p1.x,self.p1.y,self.p2.x,self.p2.y,angle/ np.pi * 180))
		return angle


class Car(object):
	def __init__(self,wide,length):
		self.wide = wide
		self.length = length

class Point2(Point):
	#坐标系单位 cm
	#中心点 车前方中央
	def __init__(self,car,L1,L2):
		self.x = 0
		self.y = 0
		w = car.wide
		self.x = (L1*L1 - L2*L2)/2/w
		y2 = L1**2 - (self.x + w/2)**2
		print("y2:%s"%y2)
		self.y = np.sqrt(y2)
		print('point x:%s y:%s'%(self.x,self.y))














if __name__ == '__main__':

	print(2**2)
	xs = np.array([721,754,326,267])

	ys = np.array([518,1074,529,1062])

	print(xs.shape)

	p = posGeter()
	#p.getPostion(xs,ys)

	L1s = [39.25,42.8,66,67.9]
	L2s = [42.5,39.2,68.2,65.9]
	ps = []
	car = Car(15,20.8)
	for i in range(4):
		p = Point2(car,L1s[i],L2s[i])
		ps.append(p)



